#ifndef A_SCRATCH_FILE_MOTOR_P1010B_111_H
#define A_SCRATCH_FILE_MOTOR_P1010B_111_H

#define  MACRO_VALUE 1
//高低字节融合整型  TODO
int16_t P1010B_Merge_uint8(uint8_t high_byte,uint8_t low_byte);

//从 16 位有符号整数中提取高字节和低字节，用于提取所需电流值的高低字节  控制电机的电流环 标识符为 0x32或者0x33  TODO
void P1010B_Split_int16(float data, uint8_t *high_byte, uint8_t *low_byte,float limit_electric);

//根据给定扭矩计算所需的电流值(此电机专属) TODO
float P1010B_CalculateCurrentFromTorque(float torque);

//根据给定电流计算所需的扭矩值(此电机专属)  TODO
float P1010B_CalculateTorqueFromCurrent(int current);

//int类型拆成四个字节，设置参数的时候需要这么操作  TODO
void P1010B_split_int_to_bytes(int32_t value, uint8_t *data);

//解析电机查询的参数，这里的参数主要指向于一些默认配置参数修改  TODO
int32_t parse_motor_data(uint8_t *DATA);

//—————————————————————————————————————————————————补偿前馈扰动的结构体——————————————————————————————————————————
// 说明: 需要集成到电机前馈控制里面，作为前馈的补偿叠加的计算的前馈控制
// DOB状态结构体
typedef struct {
    float KP_IQ;           // 比例系数用来放大负载扭矩的增益
    float Zero_IQ;         // 空载下的IQ电流需要减去以得到负载的电流
    float d_hat;           // 保存每个时刻的增益后的扭矩参数
} DOBState;

// 函数简介     负载扭矩增益结构体初始化   TODO
float P1010B_dob_update(DOBState *state,uint8_t high_byte,uint8_t low_byte);

// 函数简介     DOB计算函数(里面的参数换算仅适用于此电机，其他电机需要改变扭矩和电流的换算)  TODO
void P1010B_dob_init(DOBState *state, float kp_iq,float zero_iq);

//—————————————————————————————————————————————————补偿前馈扰动的结构体——————————————————————————————————————————


//——————————————————————————————————————查表用于tanh——————————————————————————————————————————————————

// 定义结构体：用于存储查表信息
typedef struct {
    float x_min;       // 输入范围最小值
    float x_max;       // 输入范围最大值
    int table_size;     // 实际使用的查表大小
    float tanh_table[1000]; // 静态分配的查表数据
} P1010B_TanhTable;

// 函数简介     查表法初始化(用于计算tanh函数)  TODO
void P1010B_init_tanh_table(P1010B_TanhTable *table, float min, float max, int size);

// 函数简介     非线性缩放前馈扭矩增益的最终函数，里面会调用前面的定义的函数  TODO
float compute_scaled_torque(const P1010B_TanhTable *table,float error,float error_max,float k_a,float k_out,float d_hat);
//——————————————————————————————————————查表用于tanh——————————————————————————————————————————————————


//——————————————————————————————————————PD控制，适用于此电机——————————————————————————————————————————————————
// 定义PD控制器参数结构体
typedef struct {
    float kp;        // 比例增益
    float kd;        // 阻尼增益
    float set_angle;  // 目标角度(编码器数值)
    float set_speed; // 目标速度
} P1010B_PD;

// 函数简介     基于前馈的反馈系统初始化  TODO
void P1010B_initPDController(P1010B_PD *pd, float kp, float kd);

// 函数简介     反馈系统计算函数  TODO
float P1010B_computePDControl(P1010B_PD *pd, float current_value,float current_speed);

//——————————————————————————————————————查表用于tanh——————————————————————————————————————————————————



//—————————————————————————————————————————————————电机前馈和反馈的融合设计———————————————————————————————————————————

// 函数简介     前馈配合反馈的结合计算 TODO
void Feedforward_PDC(P1010B_PD *pd,P1010B_TanhTable *table,int target_angle,int total_angle,float Actual_speed,float D_hat,uint8_t *high_byte, uint8_t *low_byte);



//—————————————————————————————————————————————————电机前馈和反馈的融合设计———————————————————————————————————————————






#if MACRO_VALUE == 1            // 这里根据头文件里面的编译来进行

#elif MACRO_VALUE == 2
// 如果 MACRO_VALUE 等于 2，则编译这里的代码
#else
// 如果 MACRO_VALUE 不等于 1 或 2，则编译这里的代码
#endif




#endif //A_SCRATCH_FILE_MOTOR_P1010B_111_H
